import numpy as np
try:
    from shapely.geometry import Polygon, Point
    from Wangfu_turning_operation import Wangfu_turning_operation
    from calculate_angle_rad_begin_to_M import calculate_angle_rad_begin_to_M
    from calculate_line_angle import calculate_line_angle
    from convert_local_path_to_latlon import convert_local_path_to_latlon
    from coordinate_transform import latlon_to_local, local_to_latlon
    from dubins import dubins_path_planning
    from find_MN import find_MN
    from find_intersection_point import find_intersection_point
    from find_point_on_line import find_point_on_line
    from fix_polygon_orientation import fix_shapely_polygon_orientation
    from insert_points_along_line import insert_points_along_line
    from order_polygon_ccw import order_polygon_ccw
    from scan_polygon_along_line import scan_polygon_with_intervals, scan_polygon_with_intervals_path
    from spiral_path_generation import spiral_path_generation
except:
    from .Wangfu_turning_operation import Wangfu_turning_operation
    from .calculate_angle_rad_begin_to_M import calculate_angle_rad_begin_to_M
    from .calculate_line_angle import calculate_line_angle
    from .convert_local_path_to_latlon import convert_local_path_to_latlon
    from .coordinate_transform import latlon_to_local, local_to_latlon
    from .dubins import dubins_path_planning
    from .find_MN import find_MN
    from .find_intersection_point import find_intersection_point
    from .find_point_on_line import find_point_on_line
    from .fix_polygon_orientation import fix_shapely_polygon_orientation
    from .insert_points_along_line import insert_points_along_line
    from .order_polygon_ccw import order_polygon_ccw
    from .scan_polygon_along_line import scan_polygon_with_intervals, scan_polygon_with_intervals_path
    from .spiral_path_generation import spiral_path_generation

class FullCoverageOmegaSteering:
    MIN_TURNING_RADIUS = 1  # 最小转弯半径 (米)
    curvature = 1/MIN_TURNING_RADIUS
    # 拖拉机参数
    TRACTOR_LENGTH = 2.8  # 车长
    TRACTOR_WIDTH = 2.2  # 车宽
    WHEEL_LEN = 0.8  # 车轮长度
    WHEEL_WIDTH = 0.2  # 车轮宽度
    def __init__(self,field_boundary_coords = [
                        [119.7347393,  32.3008734],
                        [119.7342390,  32.3006375],
                        [119.7347261,  32.2998379],
                        [119.7351718,  32.3000396]],
                        start_point = Point(119.7347393,  32.3008734),
                        start_yaw_degree = -135.0,
                        terminal_point = Point(119.7347393,  32.3008734),
                        terminal_yaw_degree = 45.0,
                        forward_interval = 0.05,
                        distance = 10,
                        MIN_TURNING_RADIUS = 7.5,
                        TRACTOR_LENGTH = 2.8,
                        TRACTOR_WIDTH = 2.2,
                        WHEEL_LEN = 0.8,
                        WHEEL_WIDTH = 0.2):
        FullCoverageOmegaSteering.MIN_TURNING_RADIUS = MIN_TURNING_RADIUS
        FullCoverageOmegaSteering.curvature = 1/FullCoverageOmegaSteering.MIN_TURNING_RADIUS
        FullCoverageOmegaSteering.TRACTOR_LENGTH = TRACTOR_LENGTH
        FullCoverageOmegaSteering.TRACTOR_WIDTH = TRACTOR_WIDTH
        FullCoverageOmegaSteering.WHEEL_LEN = WHEEL_LEN
        FullCoverageOmegaSteering.WHEEL_WIDTH = WHEEL_WIDTH

        start_point = Point(start_point)
        terminal_point = Point(terminal_point)
        # region 画地图边界
        # 重新排序坐标
        ordered_field_boundary_coords = order_polygon_ccw(field_boundary_coords, start_point)
        # 创建多边形对象
        field_boundary_poly = Polygon(ordered_field_boundary_coords)
        # 设置起点为参考点
        ref_lon = start_point.x
        ref_lat = start_point.y
        # 转换所有坐标点为局部坐标系坐标
        local_coords = [latlon_to_local(lat, lon, ref_lat, ref_lon) for lon, lat in field_boundary_poly.exterior.coords]
        # 创建局部坐标系下的多边形
        local_field_boundary_poly = Polygon(local_coords)

        self.path_points = []
        self.path_yaws = []
        # region 边界内缩和生成回字形作业参考边界

        # 不断内缩FullCoverageOmegaSteering.TRACTOR_WIDTH,生成作业带边界
        swath_boundaries = spiral_path_generation(local_field_boundary_poly, FullCoverageOmegaSteering.TRACTOR_WIDTH, FullCoverageOmegaSteering.MIN_TURNING_RADIUS)
        # 画出作业带边界
        #最内部的结束边界多边形
        local_end_boundary_poly = Polygon(swath_boundaries[-1])
        #已有的调头区宽度
        nested_turning_area_width = FullCoverageOmegaSteering.TRACTOR_WIDTH*len(swath_boundaries)

        local_Nested_operation_path_list = []
        for local_boundary in swath_boundaries:
            local_boundary_poly = Polygon(local_boundary)
            # 作业带边界外扩FullCoverageOmegaSteering.TRACTOR_WIDTH/2,得到参考作业路径
            local_Nested_operation_path_ploy = local_boundary_poly.buffer(FullCoverageOmegaSteering.TRACTOR_WIDTH/2, join_style=2, mitre_limit=5.0)
            # 换成逆时针顺序
            local_Nested_operation_path_ploy = fix_shapely_polygon_orientation(local_Nested_operation_path_ploy)

            local_Nested_operation_path_list.append([[lon,lat] for lon,lat in local_Nested_operation_path_ploy.exterior.coords])

        # region 画回字形作业路径

        local_start_x, local_start_y = latlon_to_local(start_point.y, start_point.x, ref_lat, ref_lon)
        start_yaw = np.deg2rad(start_yaw_degree)  # [rad]

        for i in range(len(local_Nested_operation_path_list)):
            #第一条作业带
            if i==0:
                for j in range(len(local_Nested_operation_path_list[i])-1):
                    if j==0:#第一条作业带第一个点
                        # 第一段曲线
                        first_x, first_y = local_Nested_operation_path_list[i][j]
                        next_x, next_y = local_Nested_operation_path_list[i][j+1]
                        next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                        end_yaw = calculate_line_angle(first_x, first_y, next_x, next_y, False)  # [rad]
                        end_x, end_y = find_point_on_line(first_x, first_y, next_x, next_y, distance)
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(local_start_x, local_start_y, start_yaw,
                                                                                            end_x, end_y, end_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points+turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = end_x
                        begin_y = end_y
                        M,N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS+0.1)

                        # 第二段直线
                        local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                        path_yaw = [ ( calculate_angle_rad_begin_to_M(begin_x,begin_y,M[0],M[1]) ) ]*len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第三段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]

                    elif not j==len(local_Nested_operation_path_list[i])-2:#第一条作业带，不是第一个点，也不是最后一个点
                        next_x, next_y = local_Nested_operation_path_list[i][j+1]
                        next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                        M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS + 0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        # 分离经度和纬度
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]
                    else:#第一条作业带最后一个点
                        current_x, current_y = local_Nested_operation_path_list[i][j]
                        #next点为两个线段延伸后的交点
                        next_x, next_y = find_intersection_point([begin_x,begin_y],local_Nested_operation_path_list[i][j + 1],local_Nested_operation_path_list[i+1][0],local_Nested_operation_path_list[i+1][1])
                        #此时，下下个点为下一条作业带的第二个点
                        next_next_x, next_next_y = local_Nested_operation_path_list[i+1][1]

                        M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS + 0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x, begin_y, M[0], M[1], forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y, False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]

            # 不是第一条作业带,也不是最后一条作业带
            elif not i==len(local_Nested_operation_path_list)-1:
                for j in range(len(local_Nested_operation_path_list[i])-1):
                    if j==0:#是当前作业带第一个点
                        first_x, first_y = local_Nested_operation_path_list[i][j]
                        next_x, next_y = local_Nested_operation_path_list[i][j+1]
                        next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]

                        M,N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS+0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        # 分离经度和纬度
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]

                    elif not j==len(local_Nested_operation_path_list[i])-2:#不是第一个点，也不是最后一个点
                        next_x, next_y = local_Nested_operation_path_list[i][j+1]
                        next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                        M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS + 0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        # 分离经度和纬度
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]
                    else:#最后一个点
                        # next点为两个线段延伸后的交点
                        next_x, next_y = find_intersection_point([begin_x, begin_y], local_Nested_operation_path_list[i][j + 1],
                                                    local_Nested_operation_path_list[i + 1][0],
                                                    local_Nested_operation_path_list[i + 1][1])
                        #此时，下下个点为下一条作业带的第二个点
                        next_next_x, next_next_y = local_Nested_operation_path_list[i+1][1]

                        M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS + 0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x, begin_y, M[0], M[1], forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        # 分离经度和纬度
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y, False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]

            # 是最后一条作业带
            else:
                for j in range(len(local_Nested_operation_path_list[i])-1):
                    if j==0:#是当前作业带第一个点
                        first_x, first_y = local_Nested_operation_path_list[i][j]
                        next_x, next_y = local_Nested_operation_path_list[i][j+1]
                        next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]

                        M,N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS+0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        # 分离经度和纬度
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]

                    elif not j==len(local_Nested_operation_path_list[i])-2:#不是第一个点，也不是最后一个点
                        next_x, next_y = local_Nested_operation_path_list[i][j+1]
                        next_next_x, next_next_y = local_Nested_operation_path_list[i][j+2]
                        M, N = find_MN(begin_x, begin_y, next_x, next_y, next_next_x, next_next_y, FullCoverageOmegaSteering.MIN_TURNING_RADIUS + 0.1)

                        # 第一段直线
                        local_forward_points = insert_points_along_line(begin_x,begin_y,M[0],M[1],forward_interval)
                        path_yaw = [(calculate_angle_rad_begin_to_M(begin_x, begin_y, M[0], M[1]))] * len(local_forward_points)
                        forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                        # 分离经度和纬度
                        self.path_points = self.path_points + forward_points
                        self.path_yaws = self.path_yaws+path_yaw

                        # 第二段曲线
                        M_yaw = calculate_line_angle(begin_x, begin_y, M[0], M[1], False)  # [rad]
                        N_yaw = calculate_line_angle(N[0], N[1], next_next_x, next_next_y,False)  # [rad]
                        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(M[0], M[1], M_yaw,
                                                                                                N[0], N[1], N_yaw, FullCoverageOmegaSteering.curvature)
                        turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                        self.path_points = self.path_points + turning_points
                        self.path_yaws = self.path_yaws+path_yaw

                        begin_x = N[0]
                        begin_y = N[1]
                    else:#最后一个点
                        first_point = [begin_x, begin_y]
                        second_point = local_Nested_operation_path_list[i][j+1]
                        # 将second_point向内部移动FullCoverageOmegaSteering.TRACTOR_WIDTH/2
                        second_point_x, second_point_y = find_point_on_line(second_point[0], second_point[1], first_point[0], first_point[1], FullCoverageOmegaSteering.TRACTOR_WIDTH/2)
                        second_point = [second_point_x, second_point_y]

        #已得到first_point和second_point
        local_intersection_points_list_path = scan_polygon_with_intervals_path(local_end_boundary_poly, first_point, second_point, FullCoverageOmegaSteering.TRACTOR_WIDTH)
        cleaned_local_intersection_points_list_path = [item for item in local_intersection_points_list_path if item]

        for i in range(len(cleaned_local_intersection_points_list_path)):
            # 如果是第一条往复线
            if i==0:
                third_point, fourth_point, local_path_x, local_path_y, turning_path_yaw, mode, lengths = Wangfu_turning_operation(cleaned_local_intersection_points_list_path[i], first_point, second_point, nested_turning_area_width, local_field_boundary_poly, FullCoverageOmegaSteering.curvature, FullCoverageOmegaSteering.TRACTOR_WIDTH)

                # 之前的那段直线
                local_forward_points = insert_points_along_line(first_point[0], first_point[1], local_path_x[0], local_path_y[0], forward_interval)
                path_yaw = [(calculate_angle_rad_begin_to_M(first_point[0], first_point[1], local_path_x[0], local_path_y[0]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                self.path_points = self.path_points + forward_points
                self.path_yaws = self.path_yaws + path_yaw

                # 第二段调头曲线
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                self.path_points = self.path_points + turning_points
                self.path_yaws = self.path_yaws + turning_path_yaw

                first_point  = [ local_path_x[-1], local_path_y[-1] ]
                second_point = fourth_point

            # 如果不是第一条往复线,也不是最后一条往复线
            elif not i==len(cleaned_local_intersection_points_list_path)-1:
                third_point, fourth_point, local_path_x, local_path_y, turning_path_yaw, mode, lengths = Wangfu_turning_operation(cleaned_local_intersection_points_list_path[i], first_point, second_point, nested_turning_area_width, local_field_boundary_poly, FullCoverageOmegaSteering.curvature, FullCoverageOmegaSteering.TRACTOR_WIDTH)

                # 之前的那段直线
                local_forward_points = insert_points_along_line(first_point[0], first_point[1], local_path_x[0], local_path_y[0], forward_interval)
                path_yaw = [(calculate_angle_rad_begin_to_M(first_point[0], first_point[1], local_path_x[0], local_path_y[0]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                self.path_points = self.path_points + forward_points
                self.path_yaws = self.path_yaws + path_yaw

                # 第二段调头曲线
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                self.path_points = self.path_points + turning_points
                self.path_yaws = self.path_yaws + turning_path_yaw


                first_point = [ local_path_x[-1], local_path_y[-1] ]
                second_point = fourth_point

            # 如果是最后一条往复线
            else:
                third_point, fourth_point, local_path_x, local_path_y, turning_path_yaw, mode, lengths = Wangfu_turning_operation(
                    cleaned_local_intersection_points_list_path[i], first_point, second_point, nested_turning_area_width,
                    local_field_boundary_poly, FullCoverageOmegaSteering.curvature, FullCoverageOmegaSteering.TRACTOR_WIDTH)

                # 之前的那段直线
                local_forward_points = insert_points_along_line(first_point[0], first_point[1], local_path_x[0],
                                                                local_path_y[0], forward_interval)
                path_yaw = [(calculate_angle_rad_begin_to_M(first_point[0], first_point[1], local_path_x[0], local_path_y[0]))] * len(local_forward_points)
                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                self.path_points = self.path_points + forward_points
                self.path_yaws = self.path_yaws + path_yaw

                # 第二段调头曲线
                turning_points = convert_local_path_to_latlon(local_path_x, local_path_y, ref_lat, ref_lon)
                self.path_points = self.path_points + turning_points
                self.path_yaws = self.path_yaws + turning_path_yaw

                # 出角度
                out_yaw = calculate_line_angle(local_path_x[-1], local_path_y[-1], fourth_point[0], fourth_point[1], False)  # [rad]
                # 终点的局部坐标
                local_terminal_x, local_terminal_y = latlon_to_local(terminal_point.y, terminal_point.x, ref_lat, ref_lon)
                # 终点角度
                terminal_yaw = np.deg2rad(terminal_yaw_degree)
                local_back_path_x, local_back_path_y, back_path_yaw, mode, lengths = dubins_path_planning(fourth_point[0], fourth_point[1], out_yaw,
                                                                                        local_terminal_x, local_terminal_y, terminal_yaw, FullCoverageOmegaSteering.curvature)

                # 最后的一段直线
                local_forward_points = insert_points_along_line(local_path_x[-1], local_path_y[-1],
                                                                fourth_point[0], fourth_point[1], forward_interval)
                path_yaw = [(calculate_angle_rad_begin_to_M(local_path_x[-1], local_path_y[-1], fourth_point[0], fourth_point[1]))] * len(local_forward_points)

                forward_points = [local_to_latlon(lon, lat, ref_lat, ref_lon) for lon, lat in local_forward_points]
                # 分离经度和纬度
                self.path_points = self.path_points + forward_points
                self.path_yaws = self.path_yaws + path_yaw

                # 回终点曲线
                turning_points = convert_local_path_to_latlon(local_back_path_x, local_back_path_y, ref_lat, ref_lon)
                self.path_points = self.path_points + turning_points
                self.path_yaws = self.path_yaws + back_path_yaw

    def get_path_points_lon_lat(self)->list:
        '''
        return [[lon1,lat1],[lon2,lat2],...]
        '''
        return self.path_points
    def get_path_points_lat_lon(self) -> list:
        '''
        return [(lat1, lon1), (lat2, lon2), ...]
        '''
        return [(point[1], point[0]) for point in self.path_points]

if __name__ == "__main__":
    a = FullCoverageOmegaSteering()
    print(a.get_path_points_lat_lon())